From 0f9e17aaa52c2f7fc586b1864c3a85156648253d Mon Sep 17 00:00:00 2001
From: Dom Cobley <popcornmix@gmail.com>
Date: Wed, 8 Jun 2022 20:49:22 +0100
Subject: [PATCH] Add HDMI1 facility to the driver.

Also check for which HDMI devices are connected and only create
devices for those that are present.

Signed-off-by: James Hughes <james.hughes@raspberrypi.org>
Signed-off-by: Dom Cobley <popcornmix@gmail.com>

snd_bcm2835: disable HDMI audio when vc4 is used (#3640)

Things don't work too well when both the vc4 driver and the firmware
driver are trying to control the same audio output:

[  763.569406] bcm2835_audio bcm2835_audio: vchi message timeout, msg=5

Hence, when the vc4 HDMI driver is used, let it control audio. This is done
by introducing a new device tree property to the audio node, and
extending the vc4-kms-v3d overlays to set it appropriately.

Signed-off-by: Hristo Venev <hristo@venev.name>

staging: bcm2835-audio: Add disable-headphones flag

Add a property to allow the headphone output to be disabled. Use an
integer property rather than a boolean so that an overlay can clear it.

Signed-off-by: Phil Elwell <phil@raspberrypi.com>

staging: bcm2835-audio: Find compatible firmware node

Commit "ARM: dts: Adopt the upstream snd_bcm2835 handling" removed the
audio section from the DT and the driver can no longer access the
referenced firmware node 'brcm,firmware'. Fix that by searching for a
compatible firmware node instead, similar to drivers/gpu/drm/vc4.

Fixes: b9e62329e096 ("ARM: dts: Adopt the upstream snd_bcm2835 handling")
Signed-off-by: Juerg Haefliger <juergh@proton.me>

staging: bcm2835-audio: Fix firmware node refcounting

Decrement firmware node refcounts on all exit paths in set_hdmi_enables().

Signed-off-by: Juerg Haefliger <juergh@proton.me>

staging: bcm2835-audio: Log errors in case of firmware query failures

The driver queries the firmware for the number of detected HDMI displays
and their IDs. Log error messages if queries fail.

Signed-off-by: Juerg Haefliger <juergh@proton.me>

staging: bcm2835-audio: Fix unused enable_hdmi module parameter

The commit "Add HDMI1 facility to the driver." made the enable_hdmi module
parameter unused. Fix that by making it a global switch for all available
HDMI audio outputs.

Fixes: 755f3366084b ("Add HDMI1 facility to the driver.")
Signed-off-by: Juerg Haefliger <juergh@proton.me>

staging: bcm2835-audio: Fix unused enable_headphones module parameter

Since commit "staging: bcm2835-audio: Add disable-headphones flag" the
enabling/disabling of the headphones output is solely determined by the
presence of the DT property 'brcm,disable-headphones' and the
enable_headphones module parameter is unused. Fix that by making it a
global switch.

Fixes: ee90e47d8824 ("staging: bcm2835-audio: Add disable-headphones flag")
Signed-off-by: Juerg Haefliger <juergh@proton.me>
---
 .../vc04_services/bcm2835-audio/bcm2835-pcm.c |   3 +-
 .../vc04_services/bcm2835-audio/bcm2835.c     | 110 ++++++++++++++++--
 .../vc04_services/bcm2835-audio/bcm2835.h     |   6 +-
 3 files changed, 105 insertions(+), 14 deletions(-)

--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835-pcm.c
@@ -321,10 +321,11 @@ static const struct snd_pcm_ops snd_bcm2
 
 /* create a pcm device */
 int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name,
-			int idx, enum snd_bcm2835_route route,
+			enum snd_bcm2835_route route,
 			u32 numchannels, bool spdif)
 {
 	struct snd_pcm *pcm;
+	int idx = chip->index++;
 	int err;
 
 	err = snd_pcm_new(chip->card, name, idx, numchannels, 0, &pcm);
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.c
@@ -8,8 +8,9 @@
 #include <linux/module.h>
 
 #include "bcm2835.h"
+#include <soc/bcm2835/raspberrypi-firmware.h>
 
-static bool enable_hdmi;
+static bool enable_hdmi, enable_hdmi0, enable_hdmi1;
 static bool enable_headphones = true;
 static int num_channels = MAX_SUBSTREAMS;
 
@@ -65,14 +66,13 @@ static int bcm2835_audio_dual_newpcm(str
 				     u32 numchannels)
 {
 	int err;
-
-	err = snd_bcm2835_new_pcm(chip, name, 0, route,
+	err = snd_bcm2835_new_pcm(chip, name, route,
 				  numchannels, false);
 
 	if (err)
 		return err;
 
-	err = snd_bcm2835_new_pcm(chip, "IEC958", 1, route, 1, true);
+	err = snd_bcm2835_new_pcm(chip, name, route, 1, true);
 	if (err)
 		return err;
 
@@ -84,20 +84,33 @@ static int bcm2835_audio_simple_newpcm(s
 				       enum snd_bcm2835_route route,
 				       u32 numchannels)
 {
-	return snd_bcm2835_new_pcm(chip, name, 0, route, numchannels, false);
+	return snd_bcm2835_new_pcm(chip, name, route, numchannels, false);
 }
 
-static struct bcm2835_audio_driver bcm2835_audio_hdmi = {
+static struct bcm2835_audio_driver bcm2835_audio_hdmi0 = {
+	.driver = {
+		.name = "bcm2835_hdmi",
+		.owner = THIS_MODULE,
+	},
+	.shortname = "bcm2835 HDMI 1",
+	.longname  = "bcm2835 HDMI 1",
+	.minchannels = 1,
+	.newpcm = bcm2835_audio_dual_newpcm,
+	.newctl = snd_bcm2835_new_hdmi_ctl,
+	.route = AUDIO_DEST_HDMI0
+};
+
+static struct bcm2835_audio_driver bcm2835_audio_hdmi1 = {
 	.driver = {
 		.name = "bcm2835_hdmi",
 		.owner = THIS_MODULE,
 	},
-	.shortname = "bcm2835 HDMI",
-	.longname  = "bcm2835 HDMI",
+	.shortname = "bcm2835 HDMI 2",
+	.longname  = "bcm2835 HDMI 2",
 	.minchannels = 1,
 	.newpcm = bcm2835_audio_dual_newpcm,
 	.newctl = snd_bcm2835_new_hdmi_ctl,
-	.route = AUDIO_DEST_HDMI
+	.route = AUDIO_DEST_HDMI1
 };
 
 static struct bcm2835_audio_driver bcm2835_audio_headphones = {
@@ -120,8 +133,12 @@ struct bcm2835_audio_drivers {
 
 static struct bcm2835_audio_drivers children_devices[] = {
 	{
-		.audio_driver = &bcm2835_audio_hdmi,
-		.is_enabled = &enable_hdmi,
+		.audio_driver = &bcm2835_audio_hdmi0,
+		.is_enabled = &enable_hdmi0,
+	},
+	{
+		.audio_driver = &bcm2835_audio_hdmi1,
+		.is_enabled = &enable_hdmi1,
 	},
 	{
 		.audio_driver = &bcm2835_audio_headphones,
@@ -268,10 +285,70 @@ static int snd_add_child_devices(struct
 	return 0;
 }
 
+static void set_hdmi_enables(struct device *dev)
+{
+	struct device_node *firmware_node;
+	struct rpi_firmware *firmware = NULL;
+	u32 num_displays, i, display_id;
+	int ret;
+
+	firmware_node = of_find_compatible_node(NULL, NULL,
+					"raspberrypi,bcm2835-firmware");
+	if (firmware_node) {
+		firmware = rpi_firmware_get(firmware_node);
+		of_node_put(firmware_node);
+	}
+
+	if (!firmware) {
+		dev_err(dev, "Failed to get fw structure\n");
+		return;
+	}
+
+	ret = rpi_firmware_property(firmware,
+				    RPI_FIRMWARE_FRAMEBUFFER_GET_NUM_DISPLAYS,
+				    &num_displays, sizeof(u32));
+	if (ret) {
+		dev_err(dev, "Failed to get fw property NUM_DISPLAYS\n");
+		goto out_rpi_fw_put;
+	}
+
+	for (i = 0; i < num_displays; i++) {
+		display_id = i;
+		ret = rpi_firmware_property(firmware,
+				RPI_FIRMWARE_FRAMEBUFFER_GET_DISPLAY_ID,
+				&display_id, sizeof(display_id));
+		if (ret) {
+			dev_err(dev, "Failed to get fw property DISPLAY_ID "
+				"(i = %d)\n", i);
+		} else {
+			if (display_id == 2)
+				enable_hdmi0 = true;
+			if (display_id == 7)
+				enable_hdmi1 = true;
+		}
+	}
+
+	if (!enable_hdmi0 && enable_hdmi1) {
+		/* Swap them over and reassign route. This means
+		 * that if we only have one connected, it is always named
+		 *  HDMI1, irrespective of if its on port HDMI0 or HDMI1.
+		 *  This should match with the naming of HDMI ports in DRM
+		 */
+		enable_hdmi0 = true;
+		enable_hdmi1 = false;
+		bcm2835_audio_hdmi0.route = AUDIO_DEST_HDMI1;
+	}
+
+out_rpi_fw_put:
+	rpi_firmware_put(firmware);
+	return;
+}
+
 static int snd_bcm2835_alsa_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	int err;
+	u32 disable_headphones = 0;
 
 	if (num_channels <= 0 || num_channels > MAX_SUBSTREAMS) {
 		num_channels = MAX_SUBSTREAMS;
@@ -279,6 +356,17 @@ static int snd_bcm2835_alsa_probe(struct
 			 num_channels);
 	}
 
+	if (enable_hdmi &&
+	    !of_property_read_bool(dev->of_node, "brcm,disable-hdmi"))
+		set_hdmi_enables(dev);
+
+	if (enable_headphones) {
+		of_property_read_u32(dev->of_node,
+				     "brcm,disable-headphones",
+				     &disable_headphones);
+		enable_headphones = !disable_headphones;
+	}
+
 	err = bcm2835_devm_add_vchi_ctx(dev);
 	if (err)
 		return err;
--- a/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
+++ b/drivers/staging/vc04_services/bcm2835-audio/bcm2835.h
@@ -33,7 +33,8 @@ enum {
 enum snd_bcm2835_route {
 	AUDIO_DEST_AUTO = 0,
 	AUDIO_DEST_HEADPHONES = 1,
-	AUDIO_DEST_HDMI = 2,
+	AUDIO_DEST_HDMI0 = 2,
+	AUDIO_DEST_HDMI1 = 3,
 	AUDIO_DEST_MAX,
 };
 
@@ -58,6 +59,7 @@ struct bcm2835_chip {
 	int volume;
 	int dest;
 	int mute;
+	int index;
 
 	unsigned int opened;
 	unsigned int spdif_status;
@@ -85,7 +87,7 @@ struct bcm2835_alsa_stream {
 
 int snd_bcm2835_new_ctl(struct bcm2835_chip *chip);
 int snd_bcm2835_new_pcm(struct bcm2835_chip *chip, const char *name,
-			int idx, enum snd_bcm2835_route route,
+			enum snd_bcm2835_route route,
 			u32 numchannels, bool spdif);
 
 int snd_bcm2835_new_hdmi_ctl(struct bcm2835_chip *chip);
